#include "mpu6050.h"

#define MPU6050_ADDRESS		0xD0		//MPU6050的I2C从机地址
//四元素解算相关
#define Kp      20.0f                        // proportional gain governs rate of convergence to accelerometer/magnetometer
#define Ki      0.0008f                      // integral gain governs rate of convergence of GYroscope biases
#define halfT   0.005f                   	 // half the sample period,sapmple freq=100Hz
#define GYRO_SPL 14.3
#define ACC_SPL  4098.0
#define ANGLE_TO_RAD(x)     ( (x) * PI / 180.0 )                                // 角度转换为弧度
#define RAD_TO_ANGLE(x)     ( (x) * 180.0 / PI )                                // 弧度转换为角度
#define PI                  ( 3.1415926535898 )

static float q0 = 1, q1 = 0, q2 = 0, q3 = 0;    // quaternion elements representing the estimated orientation
static float exInt = 0, eyInt = 0, ezInt = 0;    // scaled integral error
static float yaw_old=0,yaw_next=0,error=0,error_sum=0;
static float X_b,X_a,time_sum=0;
// 角度变量
float Roll = 0, Pitch = 0, Yaw = 0;
// 传感器原始数据变量
int16_t AX, AY, AZ, GX, GY, GZ,Gz;
float Yaw,Roll,Pitch;
// 陀螺仪偏移量变量
float GYroy_Zero;

// 常量定义
// 注释：零飘读取次数
int Zero_Times=1500;

/**
  * 函    数：MPU6050写寄存器
  * 参    数：RegAddress 寄存器地址，范围：参考MPU6050手册的寄存器描述
  * 参    数：Data 要写入寄存器的数据，范围：0x00~0xFF
  * 返 回 值：无
  */
void MPU6050_WriteReg(uint8_t RegAddress, uint8_t Data)
{
	MyI2C_Start();						//I2C起始
	MyI2C_SendByte(MPU6050_ADDRESS);	//发送从机地址，读写位为0，表示即将写入
	MyI2C_ReceiveAck();					//接收应答
	MyI2C_SendByte(RegAddress);			//发送寄存器地址
	MyI2C_ReceiveAck();					//接收应答
	MyI2C_SendByte(Data);				//发送要写入寄存器的数据
	MyI2C_ReceiveAck();					//接收应答
	MyI2C_Stop();						//I2C终止
}

/**
  * 函    数：MPU6050读寄存器
  * 参    数：RegAddress 寄存器地址，范围：参考MPU6050手册的寄存器描述
  * 返 回 值：读取寄存器的数据，范围：0x00~0xFF
  */
uint8_t MPU6050_ReadReg(uint8_t RegAddress)
{
	uint8_t Data;

	MyI2C_Start();						//I2C起始
	MyI2C_SendByte(MPU6050_ADDRESS);	//发送从机地址，读写位为0，表示即将写入
	MyI2C_ReceiveAck();					//接收应答
	MyI2C_SendByte(RegAddress);			//发送寄存器地址
	MyI2C_ReceiveAck();					//接收应答

	MyI2C_Start();						//I2C重复起始
	MyI2C_SendByte(MPU6050_ADDRESS | 0x01);	//发送从机地址，读写位为1，表示即将读取
	MyI2C_ReceiveAck();					//接收应答
	Data = MyI2C_ReceiveByte();			//接收指定寄存器的数据
	MyI2C_SendAck(1);					//发送应答，给从机非应答，终止从机的数据输出
	MyI2C_Stop();						//I2C终止

	return Data;
}

/**
  * 函    数：MPU6050初始化
  * 参    数：无
  * 返 回 值：无
  */
void MPU6050_Init(void)
{
	MyI2C_Init();									//先初始化底层的I2C
	
	/*MPU6050寄存器初始化，需要对照MPU6050手册的寄存器描述配置，此处仅配置了部分重要的寄存器*/
	MPU6050_WriteReg(MPU6050_PWR_MGMT_1, 0x01);		//电源管理寄存器1，取消睡眠模式，选择时钟源为X轴陀螺仪
	MPU6050_WriteReg(MPU6050_PWR_MGMT_2, 0x00);		//电源管理寄存器2，保持默认值0，所有轴均不待机
	MPU6050_WriteReg(MPU6050_SMPLRT_DIV, 0x09);		//采样率分频寄存器，配置采样率
	MPU6050_WriteReg(MPU6050_CONFIG, 0x06);			//配置寄存器，配置DLPF
	MPU6050_WriteReg(MPU6050_GYRO_CONFIG, 0x18);	//陀螺仪配置寄存器，选择满量程为±2000°/s
	MPU6050_WriteReg(MPU6050_ACCEL_CONFIG, 0x00);	//加速度计配置寄存器，选择满量程为±16g
}

/**
  * 函    数：MPU6050获取ID号
  * 参    数：无
  * 返 回 值：MPU6050的ID号
  */
uint8_t MPU6050_GetID(void)
{
	return MPU6050_ReadReg(MPU6050_WHO_AM_I);		//返回WHO_AM_I寄存器的值
}

/**
  * 函    数：MPU6050获取数据
  * 参    数：AccX AccY AccZ 加速度计X、Y、Z轴的数据，使用输出参数的形式返回，范围：-32768~32767
  * 参    数：GyroX GyroY GyroZ 陀螺仪X、Y、Z轴的数据，使用输出参数的形式返回，范围：-32768~32767
  * 返 回 值：无
  */
void MPU6050_GetData(int16_t *AccX, int16_t *AccY, int16_t *AccZ, 
						int16_t *GyroX, int16_t *GyroY, int16_t *GyroZ)
{
	uint8_t DataH, DataL;								//定义数据高8位和低8位的变量
	
	DataH = MPU6050_ReadReg(MPU6050_ACCEL_XOUT_H);		//读取加速度计X轴的高8位数据
	DataL = MPU6050_ReadReg(MPU6050_ACCEL_XOUT_L);		//读取加速度计X轴的低8位数据
	*AccX = (DataH << 8) | DataL;						//数据拼接，通过输出参数返回
	
	DataH = MPU6050_ReadReg(MPU6050_ACCEL_YOUT_H);		//读取加速度计Y轴的高8位数据
	DataL = MPU6050_ReadReg(MPU6050_ACCEL_YOUT_L);		//读取加速度计Y轴的低8位数据
	*AccY = (DataH << 8) | DataL;						//数据拼接，通过输出参数返回
	
	DataH = MPU6050_ReadReg(MPU6050_ACCEL_ZOUT_H);		//读取加速度计Z轴的高8位数据
	DataL = MPU6050_ReadReg(MPU6050_ACCEL_ZOUT_L);		//读取加速度计Z轴的低8位数据
	*AccZ = (DataH << 8) | DataL;						//数据拼接，通过输出参数返回
	
	DataH = MPU6050_ReadReg(MPU6050_GYRO_XOUT_H);		//读取陀螺仪X轴的高8位数据
	DataL = MPU6050_ReadReg(MPU6050_GYRO_XOUT_L);		//读取陀螺仪X轴的低8位数据
	*GyroX = (DataH << 8) | DataL;						//数据拼接，通过输出参数返回
	
	DataH = MPU6050_ReadReg(MPU6050_GYRO_YOUT_H);		//读取陀螺仪Y轴的高8位数据
	DataL = MPU6050_ReadReg(MPU6050_GYRO_YOUT_L);		//读取陀螺仪Y轴的低8位数据
	*GyroY = (DataH << 8) | DataL;						//数据拼接，通过输出参数返回
	
	DataH = MPU6050_ReadReg(MPU6050_GYRO_ZOUT_H);		//读取陀螺仪Z轴的高8位数据
	DataL = MPU6050_ReadReg(MPU6050_GYRO_ZOUT_L);		//读取陀螺仪Z轴的低8位数据
	*GyroZ = (DataH << 8) | DataL;						//数据拼接，通过输出参数返回
}

//-------------------------------------------------------------------------------------------------------------------
// 函数简介     陀螺仪偏移初始化函数
// 参数说明     无
// 返回参数     无
// 使用示例     GYroOffsetInit();
// 备注信息     该函数用于初始化陀螺仪的偏移量，通过多次读取MPU6050数据并求平均值来计算偏移。
// 备注信息     务必保证初始化时陀螺仪不被干扰！！！
//-------------------------------------------------------------------------------------------------------------------
void gyroOffsetInit(void) {
	float Sum;
    for (int i = 0; i < 600; i++) {
        MPU6050_GetData(&AX, &AY, &AZ, &GX, &GY, &GZ);
        Delay_us(1000);
        Sum += GZ;
    }
    GYroy_Zero = Sum /600;
}

//-------------------------------------------------------------------------------------------------------------------
// 函数简介     获取偏航角函数
// 参数说明     无
// 返回参数     无
// 使用示例     Get_Yaw();
// 备注信息     10ms中断，该函数通过读取MPU6050的陀螺仪数据，结合之前初始化的偏移量计算偏航角。
// 备注信息     当开机数据为0且静止时变化缓慢代表此次初始化成功，如果旋转90但变化不为90度，调整gz所乘的系数
//步骤 1：你旋转 90 度，但输出比如只有 72 度这表示偏小，需要增加修正系数,把 16.4 改成 16.4 ÷ 修正系数
//-------------------------------------------------------------------------------------------------------------------
void Get_Yaw(void) {
	
					MPU6050_GetData(&AX,&AY,&AZ,&GX,&GY,&Gz);
					Yaw+=(int)(((GYroy_Zero-Gz)/13.6)/10*10)*0.01;
					

}
//-------------------------------------------------------------------------------------------------------------------
// 函数简介     一阶卡尔曼滤波
// 参数说明     无
// 返回参数     无
// 使用示例     void MPU6050_Kalman(void);
// 备注信息     
// 备注信息     
//-------------------------------------------------------------------------------------------------------------------
int16_t Data[6];
EKF_filter ekf[6] = {{0.02,0,0,0,0.001,0.543},{0.02,0,0,0,0.001,0.543},{0.02,0,0,0,0.001,0.543},
{0.02,0,0,0,0.001,0.543},{0.02,0,0,0,0.001,0.543},{0.02,0,0.,0,0.001,0.543}};	//设置卡尔曼参数
void MPU6050_Kalman(void) 
{
	uint8_t i;
//	Data[0]=AX;
//	Data[1]=AY;
	Data[2]=AZ;
//	Data[3]=GX;
//	Data[4]=GY;
	Data[5]=GZ;

//	for(i=0;i<6;i++)
//		{ 
	    i=2;
			Kalman(&ekf[i],Data[i]);
			Data[i] = (int16_t)ekf[i].out;
		  i=5;
			Kalman(&ekf[i],Data[i]);
			Data[i] = (int16_t)ekf[i].out;
//		}
		
//	  AX  = (float)Data[0];
//	  AY  = (float)Data[1];
	  AZ  = (float)Data[2];
//	  GX  = (float)Data[3];
//	  GY  = (float)Data[4];
	  GZ  = (float)Data[5];

}	

//-------------------------------------------------------------------------------------------------------------------
// 函数简介     四元素解算
// 参数说明     无
// 返回参数     无
// 使用示例     IMU_Update();
// 备注信息     10ms中断，如果要修改采样频率要修改halfT，以及陀螺仪的采样频率MPU6050_WriteReg(MPU6050_SMPLRT_DIV, 0xC7);		//采样率分频寄存器，配置采样率
// 备注信息     陀螺仪采样频率不得低于计算频率
//-------------------------------------------------------------------------------------------------------------------
void IMU_Update(void)
{
	float gz,gy,gx,az,ay,ax;
	//可以选择用纯积分获得的偏航角，该角度为绝对角度值
	Get_Yaw();
	MPU6050_GetData(&AX, &AY, &AZ, &GX, &GY, &GZ);
	//MPU6050_Kalman();
		
	//单位转换相关
	gz=ANGLE_TO_RAD((GZ-GYroy_Zero)/GYRO_SPL/10*10);
//	gz=ANGLE_TO_RAD(GZ/GYRO_SPL);
	gy=ANGLE_TO_RAD(GY/GYRO_SPL/10*10);
	gx=ANGLE_TO_RAD(GX/GYRO_SPL/10*10);
	az=AZ/ACC_SPL;
	ay=AY/ACC_SPL;
	ax=AX/ACC_SPL;
    float norm;
    float vx, vy, vz;
    float ex, ey, ez;
    float temp0, temp1, temp2, temp3;

    float q0q0 = q0 * q0;
    float q0q1 = q0 * q1;
    float q0q2 = q0 * q2;
    //float q0q3 = q0 * q3;
    float q1q1 = q1 * q1;
    //float q1q2 = q1 * q2;
    float q1q3 = q1 * q3;
    float q2q2 = q2 * q2;
    float q2q3 = q2 * q3;
    float q3q3 = q3 * q3;

    if (ax * ay * az == 0)
    {
        return;
    }

    norm = sqrt(ax * ax + ay * ay + az * az);       //
    ax = ax / norm;
    ay = ay / norm;
    az = az / norm;

    // estimated direction of gravity and flux (v and w)
    vx = 2 * (q1q3 - q0q2);
    vy = 2 * (q0q1 + q2q3);
    vz = q0q0 - q1q1 - q2q2 + q3q3 ;

    // error is sum of cross product between reference direction of fields and direction measured by sensors
    ex = (ay * vz - az * vy) ;
    ey = (az * vx - ax * vz) ;
    ez = (ax * vy - ay * vx) ;

    exInt = exInt + ex * Ki;
    eyInt = eyInt + ey * Ki;
    ezInt = ezInt + ez * Ki;

    // adjusted GYroscope measurements
    gx = gx + Kp * ex + exInt;
    gy = gy + Kp * ey + eyInt;
    gz = gz + Kp * ez + ezInt;

    // integrate quaternion rate and normalise
    temp0 = q0;
    temp1 = q1;
    temp2 = q2;
    temp3 = q3;
    q0 += (-temp1 * gx - temp2 * gy - temp3 * gz) * halfT;
    q1 += (temp0 * gx + temp2 * gz - temp3 * gy) * halfT;
    q2 += (temp0 * gy - temp1 * gz + temp3 * gx) * halfT;
    q3 += (temp0 * gz + temp1 * gy - temp2 * gx) * halfT;

    // normalise quaternion
    norm = sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
    q0 = q0 / norm;
    q1 = q1 / norm;
    q2 = q2 / norm;
    q3 = q3 / norm;


			
    Pitch = asin(-2 * q1 * q3 + 2 * q0 * q2) * 57.3; // unit:degree
    Roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2 * q2 + 1) * 57.3; // unit:degree
}
